#pragma ident "$Id XY. CAO 2017-08-27 $"

/**
* @file SppCheck.cpp
* This is a class to compute initial value of receiver clock offset 
* and check the code observations.
*/


#include "SppCheck.hpp"
#include "constants.hpp"
#include "CProbability.hpp"

using namespace std;

namespace gpstk
{


	// Returns a string identifying this object.
	std::string SppCheck::getClassName() const
	{ return "SppCheck"; }



	/* Returns a satTypeValueMap object, adding the new data generated when
	* calling a modeling object.
	*
	* @param time      Epoch.
	* @param gData     Data object holding the data.
	*/
	satTypeValueMap& SppCheck::Process( const CommonTime& time,
		satTypeValueMap& gData )
		noexcept(false)
	{

		try
		{

			int iter = 0; TypeIDSet CheckList;
			int indexMax(-1); 
			Vector<double> solutionSPP;

			while( iter < 5 )
			{

				// Chenck satellite number
				if( gData.numSats() <= 4)
				{
					// cout << "Few sateliites at current epoch!" << endl;
					break;
				}

				// Only receiver clock and ISB are necessary if coordinate
				// is known.		
				CheckList.clear();
				CheckList.insert(TypeID::cdtSta);

				// Get a set with all satellites present in this GDS
				SatIDSet currSatSet( gData.getSatID() );

				// Judge system related bias
				bool usingGPS(false), usingGLO(false), usingGAL(false), usingBDS(false);

				// Loop through all current satellites
				for( SatIDSet::const_iterator ptr = currSatSet.begin();
					ptr != currSatSet.end();
					++ptr)
				{
					if( (*ptr).system == SatelliteSystem::GPS && (!usingGPS) )
						usingGPS = true;
					else if( (*ptr).system == SatelliteSystem::GLONASS&& (!usingGLO) )
						usingGLO = true;
					else if( (*ptr).system == SatelliteSystem::Galileo && (!usingGAL) )
						usingGAL = true;
					else if( (*ptr).system == SatelliteSystem::BDS && (!usingBDS) )
						usingBDS = true;
				}

				if( usingGPS && usingGLO )
				{
					CheckList.insert(TypeID::ISB_GLO);
				}

				if( usingGPS && usingGAL )
				{
					CheckList.insert(TypeID::ISB_GAL);
				}

				if( usingGPS && usingBDS )
				{
					CheckList.insert(TypeID::ISB_BDS);
				}

				// If no GPS, then choose GLONASS as reference
				// Add by XY. CAO, 2017-12-07
				// For real-time
				if(  (!usingGPS) && usingGLO &&  usingGAL)
				{
					CheckList.insert(TypeID::ISB_GAL);
				}

				if( (!usingGPS) && usingGLO && usingBDS )
				{
					CheckList.insert(TypeID::ISB_BDS);

				}

				// If no GPS and GLONASS, then choose GALILEO as reference
				if( (!usingGPS) &&  (!usingGLO) && usingGAL && usingBDS )
				{
					CheckList.insert(TypeID::ISB_BDS);
				}

				// Number of measurements
				int numMeasSPP( gData.numSats() ); // PC

				// Number of knowns( No geometry )
				int numUnknownSPP( CheckList.size() );

				// Build the vector of measurements (Prefit-residuals): only Code
				Vector<double> prefitSPPCode(gData.getVectorOfTypeID(TypeID::prefitC));

				// Generate the corresponding geometry/design matrix
				Matrix<double> hMatrixSPP(numMeasSPP,numUnknownSPP,0.0);
				hMatrixSPP =  gData.getMatrixOfTypes(CheckList);

				// Generate observation weight matrix
				Matrix<double> RMatrixSPP(numMeasSPP, numMeasSPP, 0.0);

                Vector<double> varianceVectorCode;
/*
				// If we have variance information, let's load it
				varianceVectorCode = (gData.getVectorOfTypeID(TypeID::varPIF12));
*/

				// Check if weights match
				if ( varianceVectorCode.size()  == numMeasSPP )
				{

					for( int i = 0; i < numMeasSPP; i++ )
					{
						RMatrixSPP( i               , i         ) = varianceVectorCode(i);
					}  // End of 'for( int i=0; i<numCurrentSV; i++ )'

				}
				else
				{
					// If weights don't match, assign generic weights
					for( int i=0; i<numMeasSPP; i++ )
					{
						RMatrixSPP( i               , i         ) = 1.0;
					}  // End of 'for( int i=0; i<numCurrentSV; i++ )'

				}  // End of 'if ( dummy.numSats() == numCurrentSV )'

				// Weights matrix
				Matrix<double> WeightMatrixSPP;
				try
				{
					// suppose sigma = 1.0
					WeightMatrixSPP =  inverseChol(RMatrixSPP);
				}
				catch(...)
				{
					// Throw an exception if something unexpectd happens
					ProcessingException e("Unable to invert SPP  Rmatrix");
					GPSTK_THROW(e);
				}

				Matrix<double> APAmatrixSPP(numUnknownSPP,numUnknownSPP,0.0);
				Matrix<double> covMatrixSPP(numUnknownSPP,numUnknownSPP,0.0);
				solutionSPP.resize(numUnknownSPP,0.0);
				Vector<double> postfitResidualsSPP(numMeasSPP,0.0);

				APAmatrixSPP = transpose(hMatrixSPP)*WeightMatrixSPP*hMatrixSPP;
				try
				{
					covMatrixSPP = inverseChol( APAmatrixSPP );
				}
				catch(...)
				{
					// Throw an exception if something unexpectd happens
					ProcessingException e("Unable to invert SPP covMatrix");
					GPSTK_THROW(e);
				}

				// Now, compute the Vector holding the solution
				solutionSPP = covMatrixSPP * transpose(hMatrixSPP)*WeightMatrixSPP*prefitSPPCode;

				// The postfit residuals Vector
				postfitResidualsSPP = hMatrixSPP * solutionSPP - prefitSPPCode;
				//cout << postfitResidualsSPP << endl;

				double freedomSPP = numMeasSPP - numUnknownSPP;

				double VVP(0.0);
				for(int i = 0; i < numMeasSPP; ++i)
				{
					VVP += postfitResidualsSPP(i)*WeightMatrixSPP(i,i)*postfitResidualsSPP(i);
				}

				// First, global test, single tailed hypothesis test
				CProbability SPP_tp;
				// alpha = 0.01
				double thresSPP0 = SPP_tp.re_chi2F(freedomSPP,0.99);
				
				// Check VPV
				if( VVP < thresSPP0 )
				{
					break;
				}

				// else , local hypothesis test
				Matrix<double> QVV = 
					RMatrixSPP - hMatrixSPP*covMatrixSPP* transpose(hMatrixSPP);

				// Suppose sigma0 is known			   
				std::vector<double> standardResidualSPP(numMeasSPP,0.0);
				for(int i = 0; i < numMeasSPP; ++i)
				{
					standardResidualSPP[i] = std::abs( postfitResidualsSPP(i) )
						/std::sqrt( QVV(i,i) );
				}

				// Find the maximum code standard residual
				std::vector<double>::iterator maxResidual = 
					std::max_element(std::begin(standardResidualSPP),
					std::end(standardResidualSPP)); 
				double standMax = (*maxResidual);

				// Find the satellite with maximum residual
				indexMax = std::distance(standardResidualSPP.begin(), maxResidual);
				SatID satDelete = ( gData.getVectorOfSatID() )(indexMax);

				CProbability t_p;
				// confidence level = 1%, much larger
				double thresSPP = t_p.re_normF(0.995); // if sigma0 known

				if( standMax > 1.5*thresSPP ) // higher because of different code precision
				{
					gData.removeSatID(satDelete);
				}
				else
				{
					break; // Must
				}

				++iter;

			} // End of loop while

			return gData;

		}   // End of try...
		catch(Exception& u)
		{
			// Throw an exception if something unexpected happens
			ProcessingException e( getClassName() + ":"
				+ u.what() );

			GPSTK_THROW(e);

		}

	}  // End of method 'SppCheck::Process()'


}  // End of namespace gpstk
